#ifndef LEG_CTRL_H
#define LEG_CTRL_H
#include <stdint.h>
#include <pthread.h>

#include "motor_ctrl.h"
#include "can_ctrl.h"
#include "ik.h"
#include "trace.h"

/* 电机构造参数描述 */
typedef struct {
    uint8_t motor_n;
    uint8_t id_array[MOTOR_NUM_MAX];
    Motor_Type type_array[MOTOR_NUM_MAX];
} Leg_Motor_Desc, *pLeg_Motor_Desc;

/* CAN构造参数描述 */
typedef struct {
    void *data;
} Leg_Can_Desc, *pLeg_Can_Desc;

/* 运动学构造参数描述 */
typedef struct {
    double l_abcd;
    double l_hip;
    double l_knee;
} Leg_IK_Desc, *pLeg_IK_Desc;

/* 轨迹构造参数描述 */
typedef struct {
    uint8_t len;
    double param[TRACE_PARAM_MAX];
    Trace_Type type;
} Leg_Trace_Desc, *pLeg_Trace_Desc;

class Leg_Ctrl {
    
public:
    Leg_Ctrl(Leg_Motor_Desc &lmotor_desc, Leg_Can_Desc &lcan_desc, Leg_IK_Desc &lik_desc, Leg_Trace_Desc &ltrace_desc);
    ~Leg_Ctrl();

    int leg_ctrl_init();
    int leg_ctrl_deinit();
    inline int is_ready() {return this->ready;}
    int32_t leg_rotate(double target_start[], double target_rotate[], double target_value[][3]);
    int32_t leg_rotate_little(double target_start[], double target_rotate[], double target_value[][3]);
    void leg_thread_enable(){this->run_flag = 1;}
    void leg_thread_disable(){this->run_flag = 0;}
    inline uint8_t leg_thread_getflag(){return run_flag;}
    friend void *leg_move_thread(void *param);
    
    Can_Ctrl    *p_can_ctrl     = nullptr;
    Motor_Ctrl  *p_motor_ctrl   = nullptr;
    IK_Obj      *p_ik_obj       = nullptr;
    Trace       *p_trace_obj    = nullptr;
protected:
    
private:
    uint8_t ready = 0;
    uint8_t run_flag = 0;
    pthread_t pid;
    
};


#endif